from PyQt5.QtWidgets import QApplication,QMainWindow
from PyQt5.QtCore import *
import rospy
from geometry_msgs.msg import PoseStamped,TwistStamped
from std_msgs.msg import String
from multiprocessing import Process,Queue

class Ros2Gui(Process):

    def __init__(self, q_text, multi_num, multi_type, flag):
        super(Ros2Gui, self).__init__()
        rospy.init_node('multirotor_pyqt5_receiver')
        self.multirotor_num = multi_num
        self.multirotor_type = multi_type
        self.local_pose_sub = [None] * self.multirotor_num
        self.local_vel_sub = [None] * self.multirotor_num
        self.local_pose = [None] * self.multirotor_num
        self.local_vel = [None] * self.multirotor_num
        self.local_pose = [PoseStamped() for i in range(self.multirotor_num)]
        self.local_vel = [TwistStamped() for i in range(self.multirotor_num)]
        self.q_text = q_text
        self.q_flag = flag
        for id_1 in range(self.multirotor_num):
            self.local_pose_sub[id_1] = rospy.Subscriber(
                self.multirotor_type + '_' + str(id_1) + "/mavros/local_position/pose", PoseStamped, self.local_pose_callback, id_1)
            self.local_vel_sub[id_1] = rospy.Subscriber(
                self.multirotor_type + '_' + str(id_1) + "/mavros/local_position/velocity_body", TwistStamped, self.local_vel_callback, id_1)
        self.run_thread()

    def run_thread(self):
        print('RUN Thread')
        self.n = 0
        flag_get = False
        rate = rospy.Rate(30)
        while True:
            self.n = self.n+1
            self.text_all = ''
            self.text = [' ' for i in range(self.multirotor_num)]
            if self.q_flag.empty():
                pass
            else:
                flag_get = self.q_flag.get()
                if flag_get:
                    rospy.signal_shutdown('STOP!')
                    break
            for id in range(self.multirotor_num):
                # if self.q_local_pose[id].empty():
                self.text[id] = 'uav pose:\n'+'uav'+str(id)+'\n'+':x:'+"%s"%(self.local_pose[id].pose.position.x)+'y:'\
                                +"%s"%(self.local_pose[id].pose.position.y)+'z:'+"%s"%(self.local_pose[id].pose.position.z)\
                                +'\n'+'uav vel:\n'+'uav'+str(id)+'\n'+':x:'+"%s"%(self.local_vel[id].twist.linear.x)+'y:'\
                                +"%s"%(self.local_vel[id].twist.linear.y)+'z:'+"%s"%(self.local_vel[id].twist.linear.z)+'\n'
                self.text_all += self.text_all + self.text[id]
            self.q_text.put(self.text_all)
            if self.n % 50 == 0:
                print 'receiver_run'
            rate.sleep()

    def local_pose_callback(self, msg, id_1):
        self.local_pose[id_1] = msg

    def local_vel_callback(self, msg, id_1):
        self.local_vel[id_1] = msg

        self.frame_3.setStyleSheet("border-image: url(/home/malan/XTDrone_my/pyqt5/images/robocup_world.jpg);")

